#ifndef __SV2_ARUCO_DET__
#define __SV2_ARUCO_DET__

#include "sms_core.h"
#include <opencv2/opencv.hpp>
#include <opencv2/aruco.hpp>
#include <opencv2/tracking.hpp>
#include <string>
#include <chrono>
#include <mutex>
#include <queue>
#include <condition_variable>
#include <Eigen/Dense>
#include "BYTETracker.h"
#define SV2_RAD2DEG 57.2957795

/*
依赖安装
sudo apt install libopencv-dev
sudo apt install libopencv-contrib-dev

*/


namespace sv2 {

using namespace cv;
using namespace cv::dnn;


class ImgMsg {
  public:
    ImgMsg(cv::Mat img_, nlohmann::json msg_) : img(img_), msg(msg_) {}
    cv::Mat img;
    nlohmann::json msg;
};

// class BYTETracker;

class MOTCppNode : public sms::BaseNode
{
public:
  MOTCppNode(
    std::string job_name,
    std::string param_file,
    std::string ip="127.0.0.1",
    int port=9094,
    nlohmann::json kwargs = {}
  ) : sms::BaseNode("MOTCppNode", job_name, param_file, ip, port, kwargs),
    _image_sub("/" + job_name + "/detector/image_results", "memory_msgs::RawImage", std::bind(&MOTCppNode::image_callback, this, std::placeholders::_1)),
    _calib_sub("/" + job_name + "/sensor/calibration_info", "sensor_msgs::CameraCalibration", std::bind(&MOTCppNode::calib_callback, this, std::placeholders::_1)),
    _trackid_filter_sub(nullptr),
    _res_pub("/" + job_name + "/mot/results", "spirecv_msgs::2DTargets"),
    _vis_pub("/" + job_name + "/mot/image_results", "memory_msgs::RawImage")
  {
    // 读取节点参数
    nlohmann::json tracked_dictionary_id = this->get_param("tracked_dictionary_id", {0});
    this->_frame_rate = this->get_param("frame_rate", 30);
    this->_track_buffer = this->get_param("track_buffer", 30);
    this->_distance_cost = this->get_param("distance_cost", "giou");
    this->_bytetrack_impl = new BYTETracker(this->_frame_rate, this->_track_buffer, this->_distance_cost);
    this->params_help(); 

    logger.info("tracked_dictionary_id: " + tracked_dictionary_id.dump());
    logger.info("frame_rate: " + std::to_string(this->_frame_rate));
    logger.info("track_buffer: " + std::to_string(this->_track_buffer));
    logger.info("distance_cost: " + this->_distance_cost);
    logger.screen_off();


    _trackid_filter_sub = new sms::Subscriber("/mate", "std_msgs::Null", std::bind(&MOTCppNode::_id_callback, this, std::placeholders::_1));
    for (const auto& item : tracked_dictionary_id) { this->_tracked_dictionary_id.push_back(item); }
    // 将读取到的参数转换到成员变量：_ids_need、_lengths_need
    this->camera_matrix = cv::Mat::zeros(3, 3, CV_64FC1);
    this->distortion = cv::Mat::zeros(1, 5, CV_64FC1);
  }
  ~MOTCppNode()
  {
    if (_trackid_filter_sub != nullptr) delete _trackid_filter_sub;
  }
  void run();
  nlohmann::json track(cv::Mat img_, nlohmann::json json_image);
  // 图像话题回调函数

  void image_callback(nlohmann::json msg)
  { 
    if (!this->_image_queue.empty())
      return;

    cv::Mat img = sms::sms2cvimg(msg);
    ImgMsg imgmsg(img, msg);

    // 放入到阻塞队列中
    {
      std::unique_lock<std::mutex> lock(this->_image_queue_mtx);
      this->_image_queue.push(imgmsg);
    }
    // 通知主线程取数据
    this->_image_cv.notify_one();
  }

  // 相机参数话题回调函数
  void calib_callback(nlohmann::json msg)
  {
    int i(0);
    for (const auto& item : msg["K"]) {this->camera_matrix.at<double>(i++) = item; }

    i = 0;
    for (const auto& item : msg["D"]) { this->distortion.at<double>(i++) = item; }
    image_width = msg["width"];
    image_height = msg["height"];

    if (!this->_first_show_calib)
    {
      this->_first_show_calib = true;
      logger.info("K: " + msg["K"].dump());
      logger.info("D: " + msg["D"].dump());
      logger.info("ImageSize: (" + std::to_string(image_width) + ", " + std::to_string(image_height) + ")");
    }   
  }

  void _id_callback(nlohmann::json msg)
  {
    if(msg.contains("selected_id"))
    {
      int selected_id = (int)msg["selected_id"];
      if (selected_id>0)
      {
        this->_selected_id = selected_id;
      }
    }
  }

  double _calculate_pixels(double center, int image_shape);

  cv::Mat camera_matrix;
  cv::Mat distortion;
  int image_width;
  int image_height;
  double fov_x;
  double fov_y;

private:
  std::vector<int> _tracked_dictionary_id;
  int _frame_rate;
  int _track_buffer;
  int _selected_id=0;
  std::string _distance_cost="giou";


  // 订阅话题
  sms::Subscriber _image_sub;
  sms::Subscriber _calib_sub;
  sms::Subscriber* _trackid_filter_sub;
  // 发布话题
  sms::Publisher _res_pub;
  sms::Publisher _vis_pub;

  BYTETracker* _bytetrack_impl;

  std::mutex _image_queue_mtx;
  std::queue<ImgMsg> _image_queue;
  std::condition_variable _image_cv;
  bool _first_show_calib;
  double _t0;

  // 发送spirecv_msgs::2DTargets话题
  nlohmann::json _publish_to_sms(
    nlohmann::json tracked_targets,
    nlohmann::json ids_msg,
    int im_width,
    int im_height
  );
};


}
#endif
